#include <vector>
#include <deque>
#include <cmath>

struct Point
{
    double x;
    double y;
    Point(double x = 0, double y = 0) : x(x), y(y) {}
};

class RealtimeSmoother
{
private:
    std::deque<Point> buffer;    // 数据缓冲区
    const int window_half_width; // 窗口半宽（实际使用左侧窗口）
    int counter = 0;             // 点计数器

public:
    RealtimeSmoother(int window_size) : window_half_width(window_size) {}

    // 添加新点并返回平滑结果
    Point addPoint(const Point &new_point)
    {
        buffer.push_back(new_point);
        ++counter;

        // 前两个点直接返回原始数据
        if (counter <= 2)
        {
            return new_point;
        }

        // 维护缓冲区大小（保留窗口所需历史数据）
        const int max_buffer_size = window_half_width + 1;
        if (buffer.size() > max_buffer_size)
        {
            buffer.pop_front();
        }

        // 执行实时平滑计算
        return calculateSmoothPoint();
    }

private:
    Point calculateSmoothPoint()
    {
        const int m = buffer.size(); // 实际窗口大小
        const int i = m - 1;         // 当前点索引（相对窗口）

        // 初始化累积量
        double sum_t = 0.0, sum_t2 = 0.0, sum_t3 = 0.0, sum_t4 = 0.0;
        double sum_x = 0.0, sum_y = 0.0;
        double sum_tx = 0.0, sum_ty = 0.0;
        double sum_t2x = 0.0, sum_t2y = 0.0;

        // 遍历窗口内所有点（相对时间坐标）
        int j = m - 5;
        if(j < 0)
        {
            j = 0;
        }
        for (; j < m; ++j)
        {
            const double t = j - i; // 时间偏移（当前点为0）
            const double t2 = t * t;
            const double t3 = t2 * t;
            const double t4 = t2 * t2;

            sum_t += t;
            sum_t2 += t2;
            sum_t3 += t3;
            sum_t4 += t4;

            const Point &p = buffer[j];
            sum_x += p.x;
            sum_y += p.y;
            sum_tx += t * p.x;
            sum_ty += t * p.y;
            sum_t2x += t2 * p.x;
            sum_t2y += t2 * p.y;
        }

        // 计算行列式
        const double D = m * (sum_t2 * sum_t4 - sum_t3 * sum_t3) - sum_t * (sum_t * sum_t4 - sum_t3 * sum_t2) + sum_t2 * (sum_t * sum_t3 - sum_t2 * sum_t2);

        if (std::abs(D) < 1e-10)
        {
            return buffer.back(); // 行列式过小返回原始值
        }

        // 计算克莱姆行列式
        const double D0_x = sum_x * (sum_t2 * sum_t4 - sum_t3 * sum_t3) - sum_tx * (sum_t * sum_t4 - sum_t3 * sum_t2) + sum_t2x * (sum_t * sum_t3 - sum_t2 * sum_t2);

        const double D0_y = sum_y * (sum_t2 * sum_t4 - sum_t3 * sum_t3) - sum_ty * (sum_t * sum_t4 - sum_t3 * sum_t2) + sum_t2y * (sum_t * sum_t3 - sum_t2 * sum_t2);

        return Point(D0_x / D, D0_y / D);
    }
};

/*
实时处理示例：*/

#include <iostream>

int main()
{
    RealtimeSmoother smoother(2); // 窗口半宽=2（实际使用3个点）

    std::vector<Point> raw_points = {{0, 0}, {1, 0.1}, {2, 0.21}, {3, 0.3}, {4, 0.3969}, {5, 0.5}, {6, 0.59}, {7, 0.79}, {8, 0.82}};

    for (auto &p : raw_points)
    {
        auto smoothed = smoother.addPoint(p);
        std::cout << "Original: (" << p.x << ", " << p.y << ")"
                  << " Smoothed: (" << smoothed.x << ", " << smoothed.y << ")\n";
    }
}
